using System;
using Microsoft.SPOT;
using GHIElectronics.NETMF.Hardware;
using Microsoft.SPOT.Hardware;
using System.Threading;

namespace AstroX
{
    public class Boom
    {
        FirgelliActuator actuator = new FirgelliActuator(AstoXPinMap.Actuator);
        OutputPort latch = new OutputPort(AstoXPinMap.LatchPin, true);

        public Boom()
        {
            FlightControl.FlightPhaseChanged += new FlightPhaseChangeHandler(FlightControl_FlightPhaseChanged);
            actuator.SetPosition = 0;
            latch.Write(false);
            Thread.Sleep(500);
            latch.Write(true);
            
        }

        public void SetPosition(int position)
        {
            actuator.SetPosition = position;
        }

        void FlightControl_FlightPhaseChanged(FlightPhases newphase)
        {
            if (newphase == FlightPhases.Extend)
            {
                FlightControl.CurrentPhase = FlightPhases.Extending;

                Open();
                
            }
            else if (newphase == FlightPhases.Retract)
            {
                FlightControl.CurrentPhase = FlightPhases.Retracting;
                Close();                
            }
            else if (newphase == FlightPhases.Retracted)
            {
                FlightControl.CurrentPhase = FlightPhases.Descent;
            }
        }


        public void Open()
        {
            Thread thread = new Thread(() =>
            {
                Program.Print("Opening latch");
                latch.Write(false);
                Thread.Sleep(10000);
                Program.Print("Opening boom");
                actuator.SetPosition = 180;
                Thread.Sleep(20 * 1000);
                Program.Print("Boom opened");
                FlightControl.CurrentPhase = FlightPhases.Extended;

            });
            thread.Start();
        }

        public void Close()
        {
            Thread thread = new Thread(() =>
           {
               Program.Print("Closing boom");
               actuator.SetPosition = 0;
               Thread.Sleep(20*1000);
               FlightControl.CurrentPhase = FlightPhases.Retracted;
               Program.Print("Closing latch");
               latch.Write(true);
               
           });
            thread.Start();
            
        }
    }
}
